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1.  Introduction 


Knowing  the  location  and  attitude  of  a  mobile  sensing  platform  is  a  prerequisite  for 
many  of  the  functions  performed  by  robots,  unmanned  aerial  vehicles,  wearable  de¬ 
vices,  game  controllers,  and  many  other  platforms.  In  many  common  applications, 
GPS  and  similar  systems  greatly  simplify  this  problem  by  providing  location  and 
velocity  information  directly.  However,  GPS  cannot  operate  indoors,  underground, 
underwater,  near  GPS  jammers,  or  on  other  planets.  These  and  other  factors  can 
deny  GPS  availability,  so  there  has  been  significant  motivation  in  recent  decades  to 
create  navigation  solutions  that  do  not  depend  on  GPS. 

One  configuration  that  has  received  considerable  attention  is  an  inertial  measure¬ 
ment  unit  (IMU)  aided  by  landmark  sightings  and  other  visual  information,  both  of 
which  existed  long  before  GPS.  Inertial  navigation  has  become  prevalent  due  to  re¬ 
cent  advances  in  microelectromechanical  system  technology,  which  have  resulted  in 
the  creation  of  chip-scale  accelerometers  and  gyroscopes.  These  devices  have  very 
low  size,  weight,  and  power  (SWaP)  requirements  and  can  be  integrated  at  a  very 
high  rate  to  provide  relative  position,  velocity,  and  attitude.  The  problem  is  that  even 
under  ideal  circumstances  the  state  estimates  from  an  IMU  diverge  as  a  function  of 
time  due  to  noise.  The  drift  is  much  greater  if  there  are  deterministic  errors  in  any 
of  the  sensor  outputs.  At  the  same  time,  extremely  low  SWaP  cameras  have  become 
ubiquitous,  and  a  host  of  computer  vision  algorithms  have  been  invented  to  identify 
points  of  interest  in  an  image,  and  track  them  reliably  from  frame  to  frame. 1-3  Uti¬ 
lizing  these  correspondences,  it  is  possible  to  aid  the  IMU  with  relative  rotation  and 
translation  information  that  drifts  as  a  function  of  what  the  camera  can  see,  rather 
than  time.  An  IMU  aided  with  computer  vision  data  for  the  purpose  of  navigation  is 
known  as  visual-inertial  navigation  system  (VINS).  Purpose-built  VINS  platforms 
exist,4  and  many  others  have  been  constructed  with  low-cost  components. 

Visual-inertial  simultaneous  localization  and  mapping  (SLAM)  and  visual-inertial 
odometry  (VIO)  are  2  closely  related  algorithm  categories  that  are  used  to  blend 
image  and  IMU  information  into  a  navigation  solution.  The  purpose  of  SLAM5-7 
is  to  estimate  the  locations  of  the  observed  landmarks  and  the  pose  of  the  vehicle 
relative  to  the  map,  whereas  VIO  uses  the  landmarks,  but  the  primary  output  is 
only  vehicle  pose  because  it  is  typically  assumed  that  there  will  be  no  opportunity 
for  loop  closure.  Both  problems  are  challenging  because  the  visual  measurements 
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are  nonlinear,  tightly  coupled  with  the  vehicle  pose,  and  usually  great  in  number. 
Despite  the  heavy  computational  cost  of  these  algorithms,  multiple  real-time  im¬ 
plementations  have  been  demonstrated. 3,8-11  However,  there  is  still  significant  room 
for  improvement  in  computational  complexity,  accuracy,  and  robustness  in  these 
algorithms. 

Early  work  in  visual  odometry  is  reviewed  succinctly  in  Scaramuzza  and  Fraun- 
dorfer12  and  Fraundorfer  and  Scaramuzza.13  Most  VIO  algorithms  proposed  in  the 
last  decade  use  either  a  Kalman  filtering  approach,  or  a  sliding-window  batch- 
optimization  approach  to  fuse  visual  correspondences  with  IMU  output.  Batch- 
optimization  approaches  require  a  history  of  past  poses  and  landmark  correspon¬ 
dences,  and  adjust  vehicle  poses,  landmark  locations,  and  usually  sensor  errors 
to  achieve  a  maximum  likelihood  estimate  (MFE)  of  these  parameters.  With  vi¬ 
sual  data,  this  is  typically  referred  to  as  bundle  adjustment  or  structure  and  motion 
(SAM).1415  This  problem  can  quickly  grow  to  unmanageable  size  for  real-time  im¬ 
plementation.  This  issue  can  be  addressed  by  using  keyframes16  and  solving  the 
MFE  incrementally  as  measurements  and  poses  are  accumulated.17'18  “Structure¬ 
less”  methods  seek  to  reduce  complexity  by  eliminating  the  need  to  estimate  land¬ 
mark  locations.  This  has  been  achieved  algebraically  by  using  3-view  constraints19 
and  by  projecting  the  residuals  onto  the  null-space  of  the  sensitivity  matrix  corre¬ 
sponding  to  landmark  location  errors.20  The  efficiency  of  using  IMU  measurements 
in  a  SAM  framework  has  been  improved  by  preintegration  techniques.20,21 

Most  recent  filter-based  methods  are  based  off  of  the  structure  of  the  multistate  con¬ 
straint  Kalman  filter  (MSCKF),22  in  which  the  strapdown  navigation  error-states  are 
augmented  by  a  sliding  window  of  “cloned”  pose  error  states.23  The  landmark  lo¬ 
cations  are  triangulated  from  the  poses  in  the  sliding  window,  and  the  correlation 
between  state  and  landmark  location  errors  is  avoided  in  the  EKF  update  by  pro¬ 
jecting  the  residuals  onto  the  null-space  of  the  sensitivity  matrix  corresponding  to 
landmark  location  errors  (i.e.,  the  residuals  are  no  longer  sensitive  to  the  landmark 
location  errors).  This  approach  has  become  very  popular  because  of  its  efficiency, 
and  many  improvements  to  the  basic  structure  have  since  been  made.  The  consis¬ 
tency  of  the  estimator  has  been  improved  by  enforcing  the  semi-group  constraint  on 
propagation  Jacobian24  and  enforcing  constraints  that  prevent  the  filter  from  gaining 
information  in  unobservable  directions25-27 
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The  optimal-state-constraint  extended  Kalman  filter  (OSC-EKF)  presented  in28  uti¬ 
lizes  an  image-only  SAM  process  over  a  fixed  window  of  previous  and  current  states 
to  estimate  the  relative  camera  poses.  This  SAM  problem  is  much  smaller  than  what 
would  be  solved  using  a  sliding  window  batch-optimization  technique,  and  can  be 
solved  efficiently  due  to  its  sparse  nature  using  various  open-source  solvers.  The 
structure  is  marginalized  out,  and  the  motion  data  is  combined  with  the  IMU  pre¬ 
dictions  using  an  EKF.  The  complexity  of  the  dense  matrix  computations  is  there¬ 
fore  only  dependent  on  the  number  of  cloned  poses,  which  is  user-selectable.  The 
prior  work  was  presented  specifically  for  an  IMU  aided  with  a  monocular  camera. 
The  novel  contribution  presented  here  is  a  generalized  version  of  the  OSC-EKF, 
that  allows  for  the  IMU  to  be  aided  by  any  measurements  that  can  produce  relative 
pose  constraints  based  on  a  MLE.  To  demonstrate  this  point,  the  algorithm  is  used 
on  the  European  Robotics  Challenge  (EuRoC)  micro  aerial  vehicle  datasets29  us¬ 
ing  both  monocular  and  stereo  configurations,  without  making  any  changes  to  the 
OSC-EKF.  Instead,  only  implementing  the  SAM  required  modification. 

2.  State  Definition 

The  OSC-EKF  algorithm  works  by  processing  the  measurements  from  several  im¬ 
age  frames  at  once  at  every  EKF  update,  as  shown  in  Fig.  1.  The  time  distance 
between  local  frames  (L0,  L\,  etc.)  is  user  defined  and  in  this  work  is  chosen  to  be 
10  camera  frames.  Between  consecutive  local  frames  (e.g.,  L0  to  L\).  the  inertial 
measurement  unit  is  integrated  to  produce  an  “IMU  trajectory”  (i.e.,  the  predicted 
state  of  the  IMU)  at  the  intermediate  image  frames  (/i,  J2,  etc.).  Concurrently,  fea¬ 
ture  points  are  tracked  between  the  image  frames  and  when  the  next  local  frame  is 
reached,  a  SAM  problem  is  solved  to  estimate  the  IMU  state  at  the  intermediate  im¬ 
age  frames,  and  the  3-D  feature  point  locations  and  other  nuisance  parameters  rela¬ 
tive  to  the  local  frame  (i.e.,  the  SAM  trajectory).  Other  relative  measurements  such 
as  wheel  odometry  or  light  detection  and  ranging  (LIDAR)  measurements  could 
be  used  as  well.  The  OSC-EKF  update  step  then  optimally  combines  the  IMU  and 
SAM  trajectories,  which  will  be  described  in  more  detail  in  the  following  sections. 
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L0  , 10  - SAM  Trajectory 

-  IMU  Trajectory 


Fig.  1  Depiction  of  OSC-EKF  window  structure 


The  state  vector  is  identical  to  other  sliding-window  EKFs  such  as  the  MSCKF, 
in  that  it  consists  of  the  current  IMU  quaternion,  velocity,  position  (^q,  Gv4  and 
Gp i  ,  respectively),  the  gyroscope,  and  accelerometer  biases  ihgk  and  bafc),  and  the 
previous  l  poses  (quaternions  and  positions).  The  total  state  vector  relative  to  the 
global  reference  frame  is 

GX  =  {Cxf  GxjjT.  (1) 

The  IMU  states  are  contained  in  Gxi,  while  the  sliding  window  of  the  previous  l 
IMU  poses  are  contained  in  Gx2: 


Xj  = 


G 


Xo  = 


Ik~1a1 

G  H 


bT  GvT  bT  Gr>T  X 

D 9k  4  “fc  P4  J  ’ 

(2) 

x  T 

GnT  ik-i^T  G  T  [ 

P4-1  •••  g  q  P4-1  j  • 

(3) 

The  OSC-EKF  utilizes  an  indirect  error-state  model;30  the  filter  estimates  error- 
states,  which  are  used  to  update  the  estimated  states  external  to  the  filter  itself.  The 
error-states  (denoted  by  •)  are  defined  by  how  they  are  applied  to  the  estimated 
states  (denoted  •)  to  obtain  the  true  states.  For  the  biases,  positions,  and  velocities, 
the  error  states  are  additive,  and  defined  as  follows: 


G 


G 


P4 

v4 


=  GP 4  +  GP4> 
=  Gv4  +  gv4, 

=  ^ gk  + 


(4) 


The  quaternions  in  this  work  use  a  multiplicative  error-state  model,  in  which  the  true 
quaternion  is  represented  by  the  estimated  quaternion,  followed  by  an  additional 
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rotation  through  an  error  quaternion: 

Gkq  (5) 

where  <8)  is  the  quaternion  multiplication  operator.  The  estimated  rotation  is  as¬ 
sumed  to  be  close  enough  to  the  true  rotation  that  the  error  quaternion  can  be  ap¬ 
proximated  by  a  vector  of  small  rotation  angles  Ik0,  which  enables  the  use  of  the 
following  approximations: 


Si  ~ 


c  39 


Ox 


C  Gfeq 


The  error-state  vector  that  is  estimated  by  the  OSC-EKF  is 


G~  _  \g^T 


x  =  rx 


2  J  • 


Where 


G- 


G- 


407’ 

b1 

9k  *k 

hTa  Gpf 

ak  *k 

6>T 

Gp  J 

^  J-k- 1 

h-reT  GpT 

(6) 

(7) 


(8) 

(9) 

(10) 


3^  Propagation 


Like  all  discrete-time  Kalman  filters,  the  OSC-EKF  requires  a  method  of  predicting 
the  current  state’s  mean  and  covariance  from  the  mean  and  covariance  at  the  pre¬ 
vious  time  step.  The  true  IMU  states  are  governed  by  continuous-time  kinematic 
relationships,  which  are  used  to  propagate  the  estimated  IMU  states,  and  linearized 
to  propagate  the  IMU  error  state  covariance.  The  sliding  window  of  previous  poses 
is  managed  by  a  process  that  has  come  to  be  called  “stochastic  cloning”.23  The 
true  IMU  states  propagate  according  to  the  following  continuous-domain  kinematic 
relationships: 


:Gq  =  n  (7u>)  £q, 


GP/  =  Gv/, 

G\i  =  Ga/; 


(11) 


bpf  YlWg, 

a  ^-wai 
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where  1  lo  is  the  angular  velocity  of  the  IMU  with  respect  to  the  global  coordinates, 
viewed  in  IMU  coordinates,  Ga j  is  the  total  acceleration  of  the  IMU,  nwg  and  nwa 
are  white  noise  processes  that  drive  the  IMU  biases,  and  the  matrix  f2  (/cu)  is  de¬ 
fined  as  follows: 


n  (Ju;) 


—  |_7^  x  J  tuj 

-!u:T  1 


(12) 


The  IMU  is  assumed  to  provide  an  angular  velocity  measurement 1  iOm  from  the  gy¬ 
roscope  and  a  specific  force  measurement  7am  from  the  accelerometer.  The  models 
for  these  measurements  are  given  by 


!ujm  =  !UJ  +  bg  +  n  g,  (13) 

7a m  =  C  (£;q)  (Ga/  -  Gg)  +  ba  +  na,  (14) 

where  Gg  is  the  gravitational  acceleration  expressed  in  the  global  frame,  and  ng  and 
na  are  white  noise  processes.  The  estimated  states  are  propagated  forward  in  time 
by  integrating  the  kinematic  equations  using  the  IMU  measurements  and  assuming 
the  error  states  and  noise  are  zero: 

1Gq  =  ^  (7d>)  £4 

GP/  =  Gv7, 

Gv/  =  Ga7,  (15) 

bs  =  0, 

ba  =  0, 

where  =  ru>m  —  b9  and  Ga /  =  C  (cq)T  —  ybj  +  Gg-  The  true  states 
and  their  derivatives  in  Eq.  11  can  be  expressed  in  terms  of  the  estimated  states 
and  error  states  according  to  their  definitions.  If  this  is  then  compared  to  Eq.  15, 
the  continuous  time  error  state  kinematics  can  be  derived.  The  reader  is  referred 
to  Trawny30  for  the  full  derivation.  The  continuous  time  error-state  kinematics  are 
represented  in  matrix  form  as 


Gxi  =  Fcgxi  +  Gcn, 


(16) 


where  n 


|  rig  n7'g  n7  n^a  j  and  the  system  matrix  and  noise  input  matrix 
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are  given  by  the  following: 


—  Ju;  xj 


-I  0 

0  0 


0 

0 


-C(^)TL/axJ  0  0  ~C£q)' 

0  0  10 
0  0  0  0 
-10  0  0 
0  1  0  0 

o  o  — c  (g0)t  0 

0  0  0  I 

0  0  0  0 


Gc  = 


(17) 


The  system  noise  is  modeled  by  uncorrelated  zero-mean  white  noise  Gaussian  pro¬ 
cesses  with  the  following  power  spectral  density  matrix: 


a2}  0  0  0 

0  a2wg  I  0  0 

0  0  a2I  0 

0  0  0  alJ 


(18) 


where  a2,  a2wg,  a2,  and  a2wa  are  the  power  spectral  densities  of  the  gyroscope  noise, 
gyroscope  bias  random  walk,  accelerometer  noise,  and  accelerometer  bias  random 
walk,  respectively.  For  this  application,  the  error-state  dynamics  are  discretized  by 
determining  the  state  transition  matrix  <I>,  and  discrete-time  error  covariance  matrix 
Q  using  the  following  definitions31: 


i-iQ 


_AfFc 


/•At 

/  e(At-T)FcGcQcG| 

Jo 


Je^-^dr 


(19) 

(20) 


where  i  is  the  current  IMU  sample  time,  and  At  is  the  IMU  sample  period.  Equa¬ 
tions  19  and  20  depend  on  the  average  accelerometer  output,  gyroscope  output,  and 
quaternion  over  the  previous  IMU  time  interval,  and  can  be  calculated  either  numer¬ 
ically32  or  using  the  closed  form  solutions  from  citeRomulouitousTR.  Typically,  the 
IMU  update  rate  is  significantly  faster  than  the  camera  frame-rate.  If  there  are  n 
IMU  samples  per  imager  sample,  then  the  following  recursive  relationships  can  be 
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used  to  determine  ,  <f>  and  j^_1Q,  the  total  state  transition  matrix  and  covariance 
matrix  between  imager  time  k  —  1  and  k: 


(k—l)n+i 

(k—l)n 

(k-l)n-\-i 

(k—l)n 


$ 

Q 


i 

i—  1 
i 

i—  1 


fo(k-l)n+i-l 
^(fc— l)n 


qu^+Uq. 


(21) 

(22) 


At  each  new  image  time  step  k,  the  current  previous  IMU  pose  becomes  the  most 
recent  pose  in  Gx2,  and  the  oldest  pose  in  Gx2  is  discarded.  This  leads  to  the  fol¬ 
lowing  linear  mapping  from  the  previous  to  the  current  error  states: 


fGxi  ,fc 
\Gx2,fc 


L 

M 

N 


£_1<E>  0i5x6Z, 

JGXi)fc_i>) 

L  M 

\G*2,k-l  J 

N 

06(iv— 1)X18 

06x6(7-1)  ®6x6 

l6(JV-l)x6(i-l)  ®6(Z-l)x6 


1-3x3 

03x3 

03x3 

03x3 

03x3 

03x3 

03x3 

03x3 

0:3x3 

1-3x3 

(23) 

(24) 

(25) 

(26) 


This  mapping  is  then  used  to  propagate  the  error-state  covariance  matrix 


k_l<&  0i5x6i 

L  M 


Pfc—  l\k— 1 


k- 1^*  ®15x6Z 

L  M 


fc—lQ  0l5x6Z 
067x15  OeixGl 


(27) 


4.  OSC-EKF  Update 

The  OSC-EKF  utilizes  the  estimates  and  Fisher  information  from  a  user-defined 
SAM  problem  in  the  EKF  update  step.  The  main  assumption  is  that  the  SAM  so¬ 
lution  provides  motion  constraints  between  the  image  frames  relative  to  some  local 
frame.  The  measurements  used  in  the  SAM  problem  are  assumed  to  have  an  output 
of  the  form 

z  —  h  (Lx,  rj)  +  v,  (28) 

where  h  is  a  vector- valued  function  of  the  IMU  poses  relative  to  the  local  frame  Lx 
and  a  collection  of  nuisance  parameters  77.  The  measurements  are  corrupted  with 
random  noise  with  assumed  distribution  v  Af(0,Rv). 
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In  the  example  provided  in  this  work,  the  measurements  are  pixel  coordinates  of 
feature  point  correspondences  tracked  by  a  pair  of  calibrated  cameras.  The  nuisance 
parameters  are  the  3-D  locations  of  those  feature  points.  However,  the  nuisance 
parameter  vector  could  easily  be  extended  to  include  camera  calibration  errors.  In 
a  different  application,  other  sensors  could  be  included  such  as  odometers,  LIDAR 
sensors,  or  baro-altimeters,  and  the  nuisance  parameter  vector  extended  to  include 
deterministic  errors  in  those  sensors.  It  is  important  however  that  the  IMU  output 
contribute  no  information  to  the  SAM  information  matrix,  because  this  information 
is  already  being  used  by  the  EKF  via  the  propagation  model.  The  IMU  poses  relative 
to  the  local  frame  are  defined  as 


=  {NT 

Lvl  ...  U-V  . 

(29) 

C(N)  =  C(§q) 

|C(gq)T. 

(30) 

Lp  4  =  C(cq) 

(GP4-GpA 

(31) 

This  definition  is  particular  to  the  convention  of  using  the  reference  frame  defined 
by  the  oldest  IMU  pose  in  the  sliding  window  as  the  local  frame.  Notice  that  if  there 
are  l  poses  in  the  sliding  window,  only  /  —  1  of  them  will  be  independent  in  the  SAM 
motion  with  relative  constraints. 

The  OSC-EKF  update  is  performed  when  the  sliding  window  has  progressed  l  im¬ 
age  samples  beyond  the  previous  update.  This  is  to  ensure  that  the  measurements 
used  in  the  EKF  update  are  uncorrelated  with  previous  measurements,  and  no  infor¬ 
mation  is  re-used.  The  SAM  problem  computes  the  maximum  likelihood  estimate 
(MLE)  of  Lx,  which  will  be  denoted  l*-mle ,  and  the  MLE  of  the  nuisance  param¬ 
eters  f]MLE.  The  predicted  local  states  Lx  can  be  computed  by  substituting  Gx  for 
Gx  in  Eqns.  30  and  31  and  this  prediction  makes  a  good  initial  guess  for  l~x.mle- 
That  is 


c([‘q)  =  c(i*q)c(gq)T,  (32) 

LPf,  =  C  (§q)  (aph  -  cpL) .  (33) 

Assuming  the  measurement  errors  in  Eq.  28  are  independent,  the  MLE  is  equivalent 
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to  the  following  nonlinear  least  squares  problem: 


rxMLE  ,  Vmle]  =  argmin  (z  -  h  (Lx,  rj))T  Rv  1  (z  -  h  (Lx,  rj))  .  (34) 

The  error-state  vector  relating  the  predicted  local  states  to  the  true  local  states  is 
given  by 


= 

{'*4?  lpI  ■ 

..  LP4_(i_1)}  » 

(35) 

ifcq  = 

(36) 

he  ** 

L  H  ~ 

{?}• 

(37) 

P4  = 

\  ) 

Lpik+Lph ■ 

(38) 

The  error  states  relating  the  MLE  local  states  to  the  true  local  states  are  denoted 
L*mle,  and  have  the  same  form.  The  problem  in  Eq.  34  is  usually  solved  using 
a  Gauss-Newton  or  Levenberg-Marquardt  iterative  approach.  The  Jacobian  of  the 
measurement  equation  with  respect  to  the  error-states  takes  on  the  form 


•  MLE 


Jl 


x MLE 


J Vmle 


(39) 


The  information  matrix  at  convergence  is  approximated  by 

H MLE  ~  J MiA  1  JmLE 

I  I io  MLE  is  asymptotically  efficient,  so  if  there  are  a  sufficient  number  of  measure¬ 
ments  it  is  reasonable  to  make  the  following  assumptions: 


Hxx 

H^x  Hfjfj 


(40) 


E  [lxMle\  ~  0,  (41) 

E  [lxmleLZmLE\  ~  (Hxx  -  HxfjH^-Hfjxj  •  (42) 

Because  the  MLE  is  also  asymptotically  normal,  it  will  be  assumed  L^MLE  ~ 
jV(0,R mle),  where  R mle  is  given  by  Eq.  42.  The  updated  can  there¬ 

fore  be  treated  as  a  direct  measurement  of  Lx  corrupted  by  zero-mean  Gaussian 
noise.  The  pose  residuals  between  lx.Mle  and  Lx  are  formulated  in  terms  of  the 
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error  states 


L^MLE®  iQ  1 > 


(43) 

(44) 


PlkMLE  P  Ik- 

The  small  rotation  angles  Ik(j)  are  computed  from  Eq.  37.  The  EKF  update  requires 
a  linear  transformation  that  maps  the  tracked  error-states  to  the  measurement  resid¬ 
uals.  The  tracked  error  states  are  Gx,  and  the  measurement  residuals  are  Lx,  so 
the  linear  transformation  can  be  found  by  applying  perturbation  analysis  to  Eqs.  30 
and  31.  It  is  helpful  to  denote  separately  the  global  error  states  for  the  local  frame 
by  position  error  GpL  and  small  angles  Li\).  Writing  the  true  states  in  terms  of  the 
estimated  and  error  states  turns  Eq.  30  into 

(i-  ['‘0xj)c(j.‘q)  ~  (i_  |^xJ)C(g§)  [(i-  [lV-xJ)c(^)]T. 

(45) 

By  applying  properties  of  the  skew- symmetric  matrix,  and  using  definition  from 
Eq.  32,  this  simplifies  to 

4</>xJ  C  (4q)  ~  [46»xJ  C  (4q)  -  |c  (M)  C  (4q)  .  (46) 

So  the  linear  transformation  for  the  small  angles  is 


In  a  similar  fashion,  Eq.  31  becomes 

lPit  +  Lph  =  (i  -  Y*  X J  )  c  (£q)  (°p/t  +  °p4  -  at>L  -  °Pt)  .  (48) 

By  using  the  definition  from  Eq.  33,  and  neglecting  products  of  error-states,  Eq.  48 
simplifies  to 

r  ,  fRP'l 

LP4~[C(M)  [LP/xJ  -C  (M)J  \  V  \  ■  (49) 

rpj 

Through  proper  arranging  of  Eqs.  47  and  49,  the  total  transformation  used  in  the 
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EKF  between  Gx  and  Lx  is  defined  by 

L±  =  Hgx.  (50) 

The  OSC-EKF  update  estimates  the  current  error  states  through  the  standard  Kalman 
update.  The  residual  covariance  is  given  by 

S  =  HPfc|fc_iH7  +  Rmle-  (51) 

The  Kalman  gain  is  calculated  as 

K  =  Pfe|fe-1HTS_1.  (52) 

The  error  states  are  calculated 

Gk  =  KLk.  (53) 

The  error-state  covariance  is  updated  by 

Pk\k  =  (I  -  KH)  Pfcifc-!  (I  -  KH)t  +  KRMLEKT.  (54) 

The  error  states  are  then  used  to  correct  the  estimated  states  using  Eqs.  4  and  5,  and 
subsequently  set  to  zero. 

5.  Practical  MLE  Issues 

One  obvious  potential  flaw  in  OSC-EKF  is  that  without  sufficient  information,  the 
MLE  can  fail  to  converge  or  converge  to  a  local  minimum.  For  example,  in  the 
stereo  vision  case  this  can  occur  if  too  few  features  are  extracted  and  matched  be¬ 
tween  cameras.  In  the  case  of  monocular  vision,  a  lack  of  motion  will  cause  the  Hes¬ 
sian  matrix  in  Eq.  40  to  become  ill-conditioned  or  singular.  Although  optimization 
methods  such  as  the  Levenberg  Marquardt  algorithm  can  compensate  for  this  and 
prevent  divergence,  a  poorly  conditioned  problem  is  less  likely  to  progress  towards 
a  well-defined  global  minimum.  The  OSC-EKF  is  not  well  suited  to  acquiring  im¬ 
ages  such  that  they  will  have  strong  geometry.14  While  the  window  of  poses  could 
be  updated  with  this  goal  in  mind  (instead  of  at  fixed  time  intervals),  the  ability 
to  do  this  is  limited  because  over  time  the  small  perturbation  assumptions  used  in 
Section  3  will  become  less  valid.  It  is  important  that  the  predicted  IMU  trajectory 
not  contribute  to  the  Fisher  information  of  the  MLE,  because  this  would  violate 
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the  EKF  assumption  that  the  state  errors  and  measurement  errors  are  independent. 
However,  it  has  been  found  that  using  the  predicted  states  and  covariances  as  priors 
in  the  MLE  estimate  can  significantly  aid  in  the  convergence  process.  Once  sta¬ 
ble  convergence  has  been  reached,  the  Fisher  information  matrix  can  be  calculated 
without  priors  for  use  in  the  EKF  update.  While  this  method  may  introduce  a  small 
bias  in  the  MLE,  it  was  found  to  significantly  improve  the  overall  performance  of 
the  algorithm,  particularly  when  using  monocular  vision. 

6.  Experimental  Results 

The  OSC-EKF  as  described  was  implemented  in  MATLAB33  and  evaluated  with 
the  EuRoC  micro  aerial  vehicle  (MAV)  datasets.29  The  datasets  were  created  by 
flying  a  AscTex  Firefly  MAV  equipped  with  2  global  shutter  greyscale  cameras  and 
an  ADIS  16448  IMU  through  2  separate  indoor  environments.  Position  and  attitude 
ground  truth  measurements  are  provided  with  the  datasets  from  a  postprocessing 
solution  aided  by  a  VICON  system  or  a  Leica  laser  tracker  depending  on  the  en¬ 
vironment.  The  IMU  data  was  provided  at  200  Hz,  and  the  image  data  at  20  Hz. 
Camera  and  IMU  calibrations  were  provided,  so  only  IMU  biases  were  included 
in  the  state  vector  as  described  in  Section  2.  IMU  sensor  noise  parameters  were 
provided  with  the  dataset,  but  they  were  not  used.  Instead,  the  discrete  state  error 
covariance  matrix  was  estimated  from  the  IMU  data  and  ground  truth  and  the  mod¬ 
eled  power  spectral  densities  adjusted  so  that  Eq.  20  produced  a  similar  matrix. 
This  noise  model  produced  more  consistent  estimates  when  incorporated  into  the 
OSC-EKF. 

A  rudimentary  image-processing  front  end  was  created.  Images  were  first  histogram- 
equilized  to  aid  in  tracking  features  in  low-light  images.  Scale  invariant  feature 
transform  (SIFT)  feature  points  were  tracked  using  the  VLFeat34  MATLAB  im¬ 
plementation.  Outliers  were  rejected  between  frames  using  RANSAC,  and  between 
cameras  using  epipolar  constraints.  The  OSC-EKF  used  a  window  size  of  10  frames. 
The  SAM  problem  was  constructed  using  an  inverse-depth  feature  position  param¬ 
eterization  as  described  in  Appendix  A.  In  the  stereo  implementation,  the  Hessian 
was  naturally  well  conditioned  because  only  features  seen  in  both  cameras  where 
included  in  the  optimization.  The  SAM  was  solved  with  a  custom  Levenberg  Mar- 
quardt  solver.  In  the  monocular  implementation,  the  convergence  was  aided  by  the 
state  prediction  as  described  in  Section  5.  In  both  cases,  residual  outliers  were  re- 
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jected  by  keeping  those  points  with  a  residual  norm  less  than  6  times  the  median 
residual  norm. 


The  algorithm  performance  from  2  of  the  dataset  trajectories  is  presented  here.  The 
“MH  05  difficult”  sequence  contained  rapid  motion  of  the  MAV  in  a  dark  machine 
hall.  The  “V2  03  difficult”  sequence  contained  rapid  motion  of  the  MAV  and  motion 
blur  in  the  images.  The  ground  truth  and  stereo  OSC-EKF  estimated  trajectories  of 
these  sequences  are  shown  in  Fig.  2.  The  actual  position  errors  are  displayed  for 
both  the  stereo  and  monocular  implementations  in  Figs.  3  and  4.  The  errors  were  all 
less  than  1  percent  of  the  distance  traveled  except  for  the  monocular  implementation 
on  the  V2  03  difficult  sequence.  The  monocular  implementations  perform  slightly 
worse  than  the  stereo  implementations,  which  is  to  be  expected  because  no  absolute 
scale  information  can  be  extracted  from  the  vision  measurements. 


Fig.  2  Stereo  position  tracking  (MH  05  difficult  left,  V2  03  difficult  right) 


Fig.  3  Position  error  for  the  MH  05  difficult  sequence  (stereo  left,  mono  right) 
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Fig.  4  Position  error  for  the  V2  03  difficult  sequence  (stereo  left,  mono  right) 


The  consistency  was  analyzed  by  examining  the  ratio  of  the  norm  of  the  position 
error  to  the  square  root  of  the  sum  of  the  position  error  covariance  terms  reported  by 
the  OSC-EKF.  Ideally,  this  ratio  should  fluctuate  around  0.65,  and  this  seems  to  be 
the  case  for  both  the  stereo  and  monocular  implementations  on  the  MH  05  difficult 
sequence  as  illustrated  in  Fig.  5.  Both  implementations  were  less  consistent  on  the 
V2  03  difficult  sequence  as  illustrated  in  Fig.  6.  It  is  likely  that  much  of  the  blame 
for  this  is  due  to  updates  in  which  very  few  feature  points  were  successfully  tracked 
for  use  in  the  SAM  problem.  The  number  of  features  used  in  each  OSC-EKF  update 
is  labeled  on  the  right  axis  of  Figs.  5  and  6.  Some  updates  in  the  V2  03  difficult 
sequence  have  fewer  than  30  useable  feature  points  with  which  to  solve  a  10-frame 
SAM  problem.  While  the  algorithm  does  not  diverge,  it  is  likely  that  the  asymptotic 
normality  assumption  is  violated  and  the  R Mle  used  to  update  the  OSC-EKF  is 
inaccurate.  Overall,  the  similarity  in  consistency  between  stereo  and  monocular 
implementations  demonstrates  the  generality  of  the  OSC-EKF  algorithm. 


Fig.  5  Consistency  analysis  for  the  MH  05  difficult  sequence  (stereo  left,  mono  right) 
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Fig.  6  Consistency  analysis  for  the  V2  03  difficult  sequence  (stereo  left,  mono  right) 


7.  Conclusions  and  Future  Work 

A  generalized  version  of  the  OSC-EKF  has  been  presented  that  can  use  any  combi¬ 
nation  of  sensors  to  create  the  relative  pose  constraints  used  in  the  update  step.  The 
implementation  is  efficient  because  Kalman  filtering  is  naturally  recursive,  and  the 
state  size  is  fixed  and  user  definable.  The  generality  of  the  algorithm  was  demon¬ 
strated  by  implementing  both  a  stereo  and  monocular  SAM  construct  with  which 
to  generate  the  relative  pose  constraints  without  changing  the  structure  of  the  OSC- 
EKF  itself.  Reasonable  drift  rates  and  consistency  were  demonstrated  on  a  challeng¬ 
ing  MAV  dataset.  The  navigation  performance  was  mainly  hindered  by  the  success 
of  the  visual  feature  tracker,  which  was  not  the  main  objective  of  this  work. 

Reliable  implementation  will  require  a  more  robust  visual  front-end  for  tracking 
features.  Efficient  execution  will  require  a  method  of  dealing  with  processing  de¬ 
lays.  The  OSC-EKF  is  actually  well  suited  to  this  because  allowing  for  delayed 
EKF  updates  would  enable  parallel  processing  of  the  OSC-EKF  propagation  and 
the  previous  SAM  problem.  Extended  experimentation  with  different  sensors,  and 
implementing  loop  closure  could  also  serve  as  future  research  directions. 


Approved  for  public  release;  distribution  is  unlimited. 


16 


8.  References 


1.  Shi  J,  Tomasi  C.  Good  features  to  track.  In:  Proc.  of  the  IEEE  Conference  on 
Computer  Vision  and  Pattern  Recognition;  1994  Jun  21-23;  Seattle,  WA.  Los 
Alamitos  (CA):  IEEE  Computer  Society;  1994.  p.  593-600. 

2.  Tuytelaars  T,  Mikolajczyk  K.  Local  invariant  feature  detectors:  a  survey. 
Found  Trends  Comput  Graph.  2008;3(3):  177-280. 

3.  Forster  C,  Pizzoli  M,  Scaramuzza  D.  SVO:  Fast  semi-direct  monocular  visual 
odometry.  In:  Proc.  of  the  IEEE  International  Conference  on  Robotics  and  Au¬ 
tomation;  2014  May  31-Jun  7;  Hong  Kong,  China.  New  York  (NY):  IEEE; 
2014. 

4.  Google.  Google  Project  Tango,  [accessed  2016  Oct  19];  https://www.google.c 
om/atap/projecttango. 

5.  Durrant- Whyte  H,  Majumder  S,  Thrun  S,  de  Battista  M,  Scheding  S.  A 
bayesian  algorithm  for  simultaneous  localisation  and  map  building.  In: 
Jarvis  RA,  Zelinsky  A,  editors.  Robotics  research:  the  tenth  international  sym¬ 
posium;  Berlin  (Germany):  Springer  Berlin  Heidelberg;  2003.  p.  49-60. 

6.  Kim  J,  Sukkarieh  S.  Real-time  implementation  of  airborne  inertial-SLAM. 
Robotics  and  Autonomous  Systems.  2007;55(1):62-71. 

7.  Li  M,  Kim  B,  Mourikis  AI.  Real-time  motion  estimation  on  a  cellphone  using 
inertial  sensing  and  a  rolling- shutter  camera.  In:  Proc.  of  the  IEEE  Interna¬ 
tional  Conference  on  Robotics  and  Automation;  2013  May  6-10;  Karlsruhe, 
Germany.  New  York  (NY):  IEEE;  2013.  p.  4697-4704. 

8.  Loianno  G,  Cross  G,  Qu  C,  Mulgaonkar  Y,  Hesch  JA,  Kumar  V.  Flying  smart¬ 
phones:  automated  flight  enabled  by  consumer  electronics.  IEEE  Robotics  Au¬ 
tomation  Magazine.  2015;22(2):24-32. 

9.  Weiss  S,  Siegwart  R.  Real-time  metric  state  estimation  for  modular  vision- 
inertial  systems.  In:  Proc.  of  the  IEEE  International  Conference  on  Robotics 
and  Automation;  2011  May  9-13;  Shanghai,  China.  New  York  (NY):  IEEE; 
2011.  p.4531-4537. 


Approved  for  public  release;  distribution  is  unlimited. 


17 


10.  Williams  S,  Indelman  V,  Kaess  M,  Roberts  R,  Leonard  JJ,  Dellaert  F.  Concur¬ 
rent  filtering  and  smoothing:  a  parallel  architecture  for  real-time  navigation  and 
full  smoothing.  International  Journal  of  Robotics  Research.  2014;33(12):1544- 
1568. 

11.  Kerl  C,  Sturm  J,  Cremers  D.  Robust  odometry  estimation  for  rgb-d  cameras. 
In:  Proc.  of  the  IEEE  International  Conference  on  Robotics  and  Automation; 
2013  May  6-10;  Karlsruhe,  Germany.  New  York  (NY):  IEEE;  2013. 

12.  Scaramuzza  D,  Fraundorfer  F.  Visual  odometry  [tutorial].  IEEE  Robotics  Au¬ 
tomation  Magazine.  201 1;18(4):80-92. 

13.  Fraundorfer  F,  Scaramuzza  D.  Visual  odometry:  Part  ii:  matching,  robust¬ 
ness,  optimization,  and  applications.  IEEE  Robotics  Automation  Magazine. 
2012;19(2):78-90. 

14.  Triggs  B,  McLauchlan  PF,  Hartley  RI,  Fitzgibbon  AW.  Bundle  adjustment  —  a 
modem  synthesis.  In:  Triggs  B,  Zisserman  A,  Szeliski  R,  editors.  Vision  Algo¬ 
rithms:  Theory  and  Practice:  International  Workshop  on  Vision  Algorithms. 
Proceedings;  2000  Sep  21-22;  Corfu,  Greece.  Berlin  (Germany):  Springer 
Berlin  Heidelberg;  2000.  p.  298-372. 

15.  Hartley  R,  Zisserman  A.  Multiple  view  geometry  in  computer  vision.  New 
York  (NY):  Cambridge  University  Press;  2004. 

16.  Leutenegger  S,  Lynen  S,  Bosse  M,  Siegwart  R,  Furgale  P.  Keyframe -based 
visual-inertial  odometry  using  nonlinear  optimization.  International  Journal  of 
Robotics  Research.  2015;34(3):3 14-334. 

17.  Kaess  M,  Ranganathan  A,  Dellaert  F.  iSAM:  incremental  smoothing  and  map¬ 
ping.  IEEE  Transactions  on  Robotics.  2008;24(6):  1365-1378. 

18.  Kaess  M,  Johannsson  H,  Roberts  R,  Ila  V,  Leonard  J,  Dellaert  F.  iSAM2: 
incremental  smoothing  and  mapping  using  the  Bayes  tree.  International  Journal 
of  Robotics  Research.  2012;31:217-236. 

19.  Indelman  V,  Dellaert  F.  Incremental  light  bundle  adjustment:  probabilistic 
analysis  and  application  to  robotic  navigation.  In:  Sun  Y,  Behai  A,  Chung  CKR, 
editors.  New  development  in  robot  vision;  Berlin  (Germany):  Springer  Berlin 
Heidelberg;  2015.  p.  111-136. 

Approved  for  public  release;  distribution  is  unlimited. 


18 


20.  Forster  C,  Carlone  L,  Dellaert  F,  Scaramuzza  D.  IMU  preintegration  on  mani¬ 
fold  for  efficient  visual-inertial  maximum-a-posteriori  estimation.  In:  Proceed¬ 
ings  of  Robotics:  Science  and  Systems;  2015  Jul  13-27;  Rome,  Italy. 

21.  Lupton  T,  Sukkarieh  S.  Visual-inertial-aided  navigation  for  high-dynamic  mo¬ 
tion  in  built  environments  without  initial  conditions.  IEEE  Transactions  on 
Robotics.  2012;28(l):61-76. 

22.  Mourikis  AI,  Roumeliotis  SI.  A  multi-state  constraint  Kalman  filter  for  vision- 
aided  inertial  navigation.  In:  Proceedings  of  the  IEEE  International  Conference 
on  Robotics  and  Automation;  2007  Apr  10-14;  Rome,  Italy.  New  York  (NY): 
IEEE;  2007.  p.  3565-3572. 

23.  Roumeliotis  SI,  Burdick  JW.  Stochastic  cloning:  a  generalized  framework  for 
processing  relative  state  measurements.  In:  Proceedings  of  the  IEEE  Interna¬ 
tional  Conference  on  Robotics  and  Automation;  2002  May  1 1-15;  Washington, 
DC.  New  York  (NY):  IEEE;  2002.  p.  1788-1795. 

24.  Huang  G,  Kaess  M,  Leonard  J.  Towards  consistent  visual-inertial  navigation. 
In:  Proc.  of  the  IEEE  International  Conference  on  Robotics  and  Automation; 
2014  May  31-Jun  7;  Hong  Kong,  China.  New  York  (NY):  IEEE;  2014.  p. 
4926-4933. 

25.  Huang  G,  Trawny  N,  Mourikis  AI,  Roumeliotis  SI.  On  the  consistency  of 
multi-robot  cooperative  localization.  In:  Proc.  of  the  Robotics:  Science  and 
Systems;  2009  Jun  31-Jul  1;  Seattle,  WA.  Cambridge,  MA:  MIT  Press;  2009. 
p.  65-72. 

26.  Hesch  JA,  Kottas  DG,  Bowman  SL,  Roumeliotis  SI.  Towards  consistent  vision- 
aided  inertial  navigation.  In:  Frazzoli  E,  Lozano-Perez  T,  Roy  N,  Rus  D,  edi¬ 
tors.  Algorithmic  foundations  of  robotics  X:  proceedings  of  the  tenth  workshop 
on  the  algorithmic  foundations  of  robotics;  Berlin  (Germany):  Springer  Berlin 
Heidelberg;  2013.  p.  559-574. 

27.  Li  M,  Mourikis  A.  High-precision,  consistent  EKF-based  visual-inertial 
odometry.  International  Journal  of  Robotics  Research.  2013;32(6):690— 7 1 1. 

28.  Huang  G,  Eckenhoff  K,  Leonard  J.  Optimal-state-constraint  EKF  for  visual- 
inertial  navigation.  In:  Proc.  of  the  International  Symposium  on  Robotics  Re¬ 
search;  2015  Sepl2-15;  Sestri  Levante,  Italy. 

Approved  for  public  release;  distribution  is  unlimited. 


19 


29.  Burn  M,  Nikolic  J,  Gohl  P,  Schneider  T,  Rehder  J,  Omari  S,  Achtelik  MW, 
Siegwart  R.  The  euroc  micro  aerial  vehicle  datasets.  The  International  Journal 
of  Robotics  Research.  2016;35(10):  1157-1 163. 

30.  Trawny  N,  Roumeliotis  SI.  Indirect  Kalman  filter  for  3D  attitude  estimation. 
Minneapolis  (MN):  University  of  Minnesota,  Dept,  of  Comp.  Sci.  &  Eng.;  2005 
Mar.  Report  No.:  2005-002,  Rev.  57. 

31.  Simon  D.  Optimal  state  estimation.  Hoboken  (NJ):  John  Wiley  and  Sons,  Inc.; 
2006. 

32.  Van  Loan  C.  Computing  integrals  involving  the  matrix  exponential.  IEEE 
Transactions  on  Automatic  Control.  1978;23(3):395^404. 

33.  MATLAB  Release  2016a.  Natick  (M A):  The  MathWorks,  Inc.;  2016. 

34.  Vedaldi  A,  Fulkerson  B.  VLFeat:  An  open  and  portable  library  of  computer 
vision  algorithms.  VLFeat.org;  2008  [accessed  2016  Apr-20],  http://www.vlfe 
at.org/. 


Approved  for  public  release;  distribution  is  unlimited. 


20 


Appendix  A.  Feature  Parameterization 
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The  example  structure  and  motion  (SAM)  problem  uses  calibrated  cameras  to  de¬ 
termine  the  maximum  likelihood  estimate  (MLE)  the  local  error  states  lx.mle  and 
nuisance  parameters  f]MLE  (feature  point  locations).  A  computer-vision  front-end 
tracks  visual  feature  points  between  consecutive  frames  and  between  cameras.  The 
measurements  used  are  the  direct  pixel  coordinates  of  these  points  from  each  cam¬ 
era;  no  prior  rectification  or  depth  estimation  is  performed  by  the  front-end.  In  pa¬ 
rameterizing  these  measurements,  it  is  useful  to  start  with  the  position  of  feature 
point  /  relative  to  the  reference  frame  attached  to  the  camera  j  at  image  time-step 
k.  denoted  Cjk 

Cj*P/  =  C  (S*q)  (c  (?q)  ("p,  -  lP4)  -  J‘P c„)  ■  (A-l) 


The  position  and  rotation  of  the  camera  relative  to  the  inertial  measurement  unit 
Ikp Cjk’  and  are  assumed  to  be  known  from  the  calibration  and  constant.  The 
feature  points  use  an  inverse-depth  parameterization  citelnverseDepth 
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P  / 


(A-2) 


The  feature  point  in  the  left  camera  frame  then  becomes 
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The  term  in  the  brackets  can  be  replaced  by  the  nonlinear  functions  hx,  hy .  hz 


c,,p  t 
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Pi 
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hy  (af, /3f,pf, [kq,LpIk 
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(A-4) 
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The  normalized  pixel  coordinates  {u,  v},  are  then  calculated  as 


hz(a},Ps,ps^  qTp/J  I 
hv(af^f’Pf^L^LPkl)  | 

hz{af,pf,ps!^ qTp/fc)  J 


(A- 5) 


Equation  A-5  is  the  general  form  used  to  calculate  normalized  pixel  coordinates  for 
every  feature  point  observed  in  the  SAM  window  in  both  cameras. 
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List  of  Symbols,  Abbreviations,  and  Acronyms 


3-D 

3-dimensional. 

BA 

bundle  adjustment. 

EKF 

extended  Kalman  filter. 

GPS 

global  positioning  system. 

IMU 

inertial  measurement  unit. 

LIDAR 

light  detection  and  ranging. 

MAV 

micro  aerial  vehicle. 

MLE 

maximum  likelihood  estimate. 

MSCKF 

multiple  state  constraint  filter. 

OSC 

optimal  state  constraint. 

SAM 

structure  and  motion. 

SLAM 

simultaneous  localization  and  mapping. 

SWaP 

size,  weight,  and  power. 

VINS 

visual  inertial  navigation  system. 

VIO 

visual  inertial  odometry. 
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